作動関節マニピュレータの逆運動学

Created by Masahiro Furukawa, Aug 18, 2020

#参考URL -> https://qiita.com/tibigame/items/61cecf86fc978628bfee
#参考図書 -> ポールのロボット・マニピュレータ

import numpy as np
import sympy as sym
sym.init_printing()
Pi = sym.S.Pi # 円周率
E = sym.S.Exp1 # 自然対数の底
I = sym.S.ImaginaryUnit # 虚数単位

#sympyの円周率の方を使うことをすすめる（こっちの方が量子化誤差が大きくなる．numpyも同様に大きい）
import math
pi = math.pi

# 角度変数
(J_1,J_2,J_3,J_4,J_5,J_6) = sym.symbols('J_1,J_2,J_3,J_4,J_5,J_6')

# リンクパラメータ
(a_1,a_2,a_3,d_4) = sym.symbols('a_1,a_2,a_3,d_4')

# リンクパラメータ
(j,a,d,alpha) = sym.symbols('j,a,d,alpha')

# T6
(n_x, n_y, n_z, o_x, o_y, o_z, a_x, a_y, a_z, p_x, p_y, p_z) = sym.symbols('n_x, n_y, n_z, o_x, o_y, o_z, a_x, a_y, a_z, p_x, p_y, p_z')

#sin.cosの簡易記述用
def S(a):
    return sym.sin(a)

def C(a):
    return sym.cos(a)

#回転・並進行列
def rotx(a):
    return sym.Matrix([[1, 0, 0, 0], [0, C(a), -S(a), 0], [0, S(a), C(a), 0], [0, 0, 0, 1]])

def roty(a):
    return sym.Matrix([[C(a), 0, S(a), 0], [0, 1, 0, 0], [-S(a), 0, C(a), 0], [0, 0, 0, 1]])

def rotz(a):
    return sym.Matrix([[C(a), -S(a), 0, 0], [S(a), C(a), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])

def trans(x, y, z):
    return sym.Matrix([[1, 0, 0, x], [0, 1, 0, y], [0, 0, 1, z], [0, 0, 0, 1]])

# DH matrix 
def DH(j, alpha, a, d):
    return rotz(j)*trans(a,0,d)*rotx(alpha)

# inverse DH matrix
def DHi(j, alpha, a, d):
    return rotx(-alpha)*trans(-a,0,-d)*rotz(-j)

DH法に基づく座標系間の関係表現

座標系 i	Z_i-1軸回りに角度θ_i	X_i軸周りにねじれ角α_iだけ回転	回転後のX_i-1 (=X_i)に沿って長さa_iだけ並進	Z_i-1に沿って距離d_iだけ並進
1	$J_1$	$\pi/2$	$a_1$	0
2	$J_2+\pi/2$	0	$a_2$	0
3	$J_3-J_2$	$\pi/2$	$a_3$	0
4	$J_4$	$-\pi/2$	0	$d_4$
5	$J_5$	$\pi/2$	0	0
6	$J_6$	0	0	0	$

変換行列 A

easy=False
if(easy):
    A1=sym.trigsimp( DH (J_1,             Pi/2, 0, 0))
    A3=sym.trigsimp( DH (J_3 - J_2,   Pi/2, 0, 0))
    A1i=sym.trigsimp( DHi (J_1,             Pi/2, 0, 0))
    A3i=sym.trigsimp( DHi (J_3 - J_2,   Pi/2, 0, 0))
else:
    A1=sym.trigsimp( DH (J_1,             Pi/2, a_1, 0))
    A3=sym.trigsimp( DH (J_3 - J_2,   Pi/2, a_3, 0))
    # inverse matrix
    A1i=sym.trigsimp( DHi (J_1,             Pi/2, a_1, 0))
    A3i=sym.trigsimp( DHi (J_3 - J_2,   Pi/2, a_3, 0))

A2=sym.trigsimp( DH (J_2+ Pi/2,       0, a_2, 0))
A4=sym.trigsimp( DH (J_4,            -Pi/2,      0, d_4))
A5=sym.trigsimp( DH (J_5,             Pi/2,      0, 0))
A6=sym.trigsimp( DH (J_6,                   0,     0, 0))

# inverse matrix
A2i=sym.trigsimp( DHi (J_2+ Pi/2,       0, a_2, 0))
A4i=sym.trigsimp( DHi (J_4,            -Pi/2,      0, d_4))
A5i=sym.trigsimp( DHi (J_5,             Pi/2,      0, 0))
A6i=sym.trigsimp( DHi (J_6,                   0,     0, 0))

A1

$\displaystyle \left[\begin{matrix}\cos{\left(J_{1} \right)} & 0 & \sin{\left(J_{1} \right)} & a_{1} \cos{\left(J_{1} \right)}\\\sin{\left(J_{1} \right)} & 0 & - \cos{\left(J_{1} \right)} & a_{1} \sin{\left(J_{1} \right)}\\0 & 1 & 0 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

# 逆行列をかけると単位行列になることの確認
ret = A2i*A2
sym.trigsimp(ret)

$\displaystyle \left[\begin{matrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

逆運動学

T6=sym.Matrix([[n_x, o_x, a_x, p_x], [n_y, o_y, a_y, p_y], [n_z, o_z, a_z, p_z], [0, 0, 0, 1]])
T6

$\displaystyle \left[\begin{matrix}n_{x} & o_{x} & a_{x} & p_{x}\\n_{y} & o_{y} & a_{y} & p_{y}\\n_{z} & o_{z} & a_{z} & p_{z}\\0 & 0 & 0 & 1\end{matrix}\right]$

# forward kinematics
A56 = sym.trigsimp( A5*A6 )
A456 = sym.trigsimp( A4*A5*A6 )
A3456 = sym.trigsimp( A3*A4*A5*A6 )
A23456 = sym.trigsimp( A2*A3*A4*A5*A6 )
T  = sym.trigsimp( A1*A2*A3*A4*A5*A6 )

T

$\displaystyle \left[\begin{matrix}\left(\left(\sin{\left(J_{1} \right)} \sin{\left(J_{4} \right)} - \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)}\right) \cos{\left(J_{5} \right)} - \sin{\left(J_{5} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)}\right) \cos{\left(J_{6} \right)} + \left(\sin{\left(J_{1} \right)} \cos{\left(J_{4} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)}\right) \sin{\left(J_{6} \right)} & - \left(\left(\sin{\left(J_{1} \right)} \sin{\left(J_{4} \right)} - \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)}\right) \cos{\left(J_{5} \right)} - \sin{\left(J_{5} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)}\right) \sin{\left(J_{6} \right)} + \left(\sin{\left(J_{1} \right)} \cos{\left(J_{4} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)}\right) \cos{\left(J_{6} \right)} & \left(\sin{\left(J_{1} \right)} \sin{\left(J_{4} \right)} - \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)}\right) \sin{\left(J_{5} \right)} + \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} & \left(a_{1} - a_{2} \sin{\left(J_{2} \right)} - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)}\right) \cos{\left(J_{1} \right)}\\\left(- \left(\sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} + \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)}\right) \cos{\left(J_{5} \right)} - \sin{\left(J_{1} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)}\right) \cos{\left(J_{6} \right)} + \left(\sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} - \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)}\right) \sin{\left(J_{6} \right)} & - \left(- \left(\sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} + \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)}\right) \cos{\left(J_{5} \right)} - \sin{\left(J_{1} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)}\right) \sin{\left(J_{6} \right)} + \left(\sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} - \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)}\right) \cos{\left(J_{6} \right)} & - \left(\sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} + \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)}\right) \sin{\left(J_{5} \right)} + \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} & \left(a_{1} - a_{2} \sin{\left(J_{2} \right)} - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)}\right) \sin{\left(J_{1} \right)}\\\left(- \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}\right) \cos{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{3} \right)} & - \left(- \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}\right) \sin{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{6} \right)} & \sin{\left(J_{3} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} & a_{2} \cos{\left(J_{2} \right)} + a_{3} \cos{\left(J_{3} \right)} + d_{4} \sin{\left(J_{3} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$

順運動学計算用Cソースコードを得る

# Masahiro Furukawa
# Aug, 17, 2020
#
# refernce : https://qiita.com/JmpM/items/4bea4997aaf406cca3b4

# Cソースを得る
for ii in range(4):
    for jj in range(4):
        idx = jj*4+ii
        code = sym.ccode(T[idx],assign_to=('Trans['+str(jj)+']['+str(ii)+']'), standard='C89')
        print(code)
    print()

Trans[0][0] = ((sin(J_1)*sin(J_4) - sin(J_3)*cos(J_1)*cos(J_4))*cos(J_5) - sin(J_5)*cos(J_1)*cos(J_3))*cos(J_6) + (sin(J_1)*cos(J_4) + sin(J_3)*sin(J_4)*cos(J_1))*sin(J_6);
Trans[1][0] = (-(sin(J_1)*sin(J_3)*cos(J_4) + sin(J_4)*cos(J_1))*cos(J_5) - sin(J_1)*sin(J_5)*cos(J_3))*cos(J_6) + (sin(J_1)*sin(J_3)*sin(J_4) - cos(J_1)*cos(J_4))*sin(J_6);
Trans[2][0] = (-sin(J_3)*sin(J_5) + cos(J_3)*cos(J_4)*cos(J_5))*cos(J_6) - sin(J_4)*sin(J_6)*cos(J_3);
Trans[3][0] = 0;
()
Trans[0][1] = -((sin(J_1)*sin(J_4) - sin(J_3)*cos(J_1)*cos(J_4))*cos(J_5) - sin(J_5)*cos(J_1)*cos(J_3))*sin(J_6) + (sin(J_1)*cos(J_4) + sin(J_3)*sin(J_4)*cos(J_1))*cos(J_6);
Trans[1][1] = -(-(sin(J_1)*sin(J_3)*cos(J_4) + sin(J_4)*cos(J_1))*cos(J_5) - sin(J_1)*sin(J_5)*cos(J_3))*sin(J_6) + (sin(J_1)*sin(J_3)*sin(J_4) - cos(J_1)*cos(J_4))*cos(J_6);
Trans[2][1] = -(-sin(J_3)*sin(J_5) + cos(J_3)*cos(J_4)*cos(J_5))*sin(J_6) - sin(J_4)*cos(J_3)*cos(J_6);
Trans[3][1] = 0;
()
Trans[0][2] = (sin(J_1)*sin(J_4) - sin(J_3)*cos(J_1)*cos(J_4))*sin(J_5) + cos(J_1)*cos(J_3)*cos(J_5);
Trans[1][2] = -(sin(J_1)*sin(J_3)*cos(J_4) + sin(J_4)*cos(J_1))*sin(J_5) + sin(J_1)*cos(J_3)*cos(J_5);
Trans[2][2] = sin(J_3)*cos(J_5) + sin(J_5)*cos(J_3)*cos(J_4);
Trans[3][2] = 0;
()
Trans[0][3] = (a_1 - a_2*sin(J_2) - a_3*sin(J_3) + d_4*cos(J_3))*cos(J_1);
Trans[1][3] = (a_1 - a_2*sin(J_2) - a_3*sin(J_3) + d_4*cos(J_3))*sin(J_1);
Trans[2][3] = a_2*cos(J_2) + a_3*cos(J_3) + d_4*sin(J_3);
Trans[3][3] = 1;
()

inverse kinematics

$$ % reference : https://qiita.com/namoshika/items/63db972bfd1030f8264a % 空白は表示に影響しない。コメントは"%"で始める % 下付き文字は"_a"、上付き文字は"^a" % 改行は"\\"を付ける {\boldsymbol{A}_{1}}^{-1} \boldsymbol{T}_6 = \boldsymbol{A}_2 \boldsymbol{A}_3 \boldsymbol{A}_4 \boldsymbol{A}_5 \boldsymbol{A}_6 　　　(3.75)\\ {\boldsymbol{A}_{1}}^{-1} \boldsymbol{T}_6 = ^{1}\boldsymbol{T}_6 　　　(3.76) % 複数文字を1要素とする際は{...}で囲う % 空白は"\quad" $$

T16 = sym.trigsimp(                         A1i*T6 ) #  eq(3.70)
T26 = sym.trigsimp(                   A2i*A1i*T6 ) #  eq(3.71)
T36 = sym.trigsimp(             A3i*A2i*A1i*T6 ) #  eq(3.72)
T46 = sym.trigsimp(       A4i*A3i*A2i*A1i*T6 ) #  eq(3.73)
T56 = sym.trigsimp( A5i*A4i*A3i*A2i*A1i*T6 ) #  eq(3.74)

# Left hand  of (3.76)
A1iT6 = T16
A1iT6

$\displaystyle \left[\begin{matrix}n_{x} \cos{\left(J_{1} \right)} + n_{y} \sin{\left(J_{1} \right)} & o_{x} \cos{\left(J_{1} \right)} + o_{y} \sin{\left(J_{1} \right)} & a_{x} \cos{\left(J_{1} \right)} + a_{y} \sin{\left(J_{1} \right)} & - a_{1} + p_{x} \cos{\left(J_{1} \right)} + p_{y} \sin{\left(J_{1} \right)}\\n_{z} & o_{z} & a_{z} & p_{z}\\n_{x} \sin{\left(J_{1} \right)} - n_{y} \cos{\left(J_{1} \right)} & o_{x} \sin{\left(J_{1} \right)} - o_{y} \cos{\left(J_{1} \right)} & a_{x} \sin{\left(J_{1} \right)} - a_{y} \cos{\left(J_{1} \right)} & p_{x} \sin{\left(J_{1} \right)} - p_{y} \cos{\left(J_{1} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$

# Right hand of (3.76)
A23456

$\displaystyle \left[\begin{matrix}- \left(\sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)}\right) \cos{\left(J_{6} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} & \left(\sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)}\right) \sin{\left(J_{6} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{6} \right)} & - \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} & - a_{2} \sin{\left(J_{2} \right)} - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)}\\\left(- \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}\right) \cos{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{3} \right)} & - \left(- \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}\right) \sin{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{6} \right)} & \sin{\left(J_{3} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} & a_{2} \cos{\left(J_{2} \right)} + a_{3} \cos{\left(J_{3} \right)} + d_{4} \sin{\left(J_{3} \right)}\\\sin{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)} + \sin{\left(J_{6} \right)} \cos{\left(J_{4} \right)} & - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{5} \right)} + \cos{\left(J_{4} \right)} \cos{\left(J_{6} \right)} & \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

# eq2 (3.76)
sym.Eq(A1iT6[11] , A23456[11])

$\displaystyle p_{x} \sin{\left(J_{1} \right)} - p_{y} \cos{\left(J_{1} \right)} = 0$

$$ \displaystyle \frac{ \sin{\left(J_{1} \right)} }{ \cos{\left(J_{1} \right)} } = \frac{p_{y}}{p_{x}} \\ \rightarrow \displaystyle \tan{\left({ J_{1} }\right)} = \frac{p_{y}}{p_{x}} \\ $$

Therefore $$\rightarrow \displaystyle J_{1} = atan2{\left({p_{y}},{p_{x}}\right)} \\ $$

display( sym.simplify(  sym.Eq(T6[11] , T[11]) ) )

print("therefore, ")
eq_C2 = ( p_z - a_3*C(J_3)-d_4*S(J_3) ) /a_2 

display( sym.Eq(C(J_2), eq_C2 ) )

print("If ( 1 - (p_z - a_3 * C(J_3) - d_4 * S(J_3) )^2 < 0, then no answer.\n")

print("If ( 1 - (p_z - a_3 * C(J_3) - d_4 * S(J_3) )^2 ==0 , then ")

display( sym.Eq( - a_3 * C(J_3) - d_4 * S(J_3), 1 - p_z) )

print("Solusion on J_3 equals to the below")

sol =  sym.solve( sym.Eq( - a_3 * C(J_3) - d_4 * S(J_3), 1-p_z),  J_3 )

display(sym.Eq(J_3, sol[0]) ,sym.Eq(J_3, sol[1]) )

print("If ( 1 - (p_z - a_3 * C(J_3) - d_4 * S(J_3) )^2 >0 , then ")

eq_S2p =  (sym.sqrt( 1 - ( p_z - a_3*C(J_3)-d_4*S(J_3) )*( p_z - a_3*C(J_3)-d_4*S(J_3) )) ) / a_3
eq_S2n = -eq_S2p

display( sym.Eq(S(J_2), eq_S2n ) )

print("Here is the equation below")

display( sym.collect( sym.Eq(A1iT6[3] , A23456[3]) , S(J_3) ) )
display( sym.collect( sym.Eq(A1iT6[7] , A23456[7]) , S(J_3) ) )
display( sym.Eq(S(J_2), eq_S2p ) )

$\displaystyle p_{z} = a_{2} \cos{\left(J_{2} \right)} + a_{3} \cos{\left(J_{3} \right)} + d_{4} \sin{\left(J_{3} \right)}$

therefore, 

$\displaystyle \cos{\left(J_{2} \right)} = \frac{- a_{3} \cos{\left(J_{3} \right)} - d_{4} \sin{\left(J_{3} \right)} + p_{z}}{a_{2}}$

If ( 1 - (p_z - a_3 * C(J_3) - d_4 * S(J_3) )^2 < 0, then no answer.

If ( 1 - (p_z - a_3 * C(J_3) - d_4 * S(J_3) )^2 ==0 , then 

$\displaystyle - a_{3} \cos{\left(J_{3} \right)} - d_{4} \sin{\left(J_{3} \right)} = 1 - p_{z}$

Solusion on J_3 equals to the below

$\displaystyle J_{3} = 2 \operatorname{atan}{\left(\frac{d_{4} - \sqrt{a_{3}^{2} + d_{4}^{2} - p_{z}^{2} + 2 p_{z} - 1}}{a_{3} + p_{z} - 1} \right)}$

$\displaystyle J_{3} = 2 \operatorname{atan}{\left(\frac{d_{4} + \sqrt{a_{3}^{2} + d_{4}^{2} - p_{z}^{2} + 2 p_{z} - 1}}{a_{3} + p_{z} - 1} \right)}$

If ( 1 - (p_z - a_3 * C(J_3) - d_4 * S(J_3) )^2 >0 , then 

$\displaystyle \sin{\left(J_{2} \right)} = - \frac{\sqrt{1 - \left(- a_{3} \cos{\left(J_{3} \right)} - d_{4} \sin{\left(J_{3} \right)} + p_{z}\right)^{2}}}{a_{3}}$

$\displaystyle \sin{\left(J_{2} \right)} = \frac{\sqrt{1 - \left(- a_{3} \cos{\left(J_{3} \right)} - d_{4} \sin{\left(J_{3} \right)} + p_{z}\right)^{2}}}{a_{3}}$

eq3 = A23456[3] - A1iT6[3]
eq7 = A23456[7] - A1iT6[7]

print("Here is the equation below\n eq3 = ")
display( sym.collect(eq3, S(J_3) ) )
print("eq7 = ")
display( sym.collect(eq7, S(J_3) ) )

(H3, K3, A3, B3, C3, D3) = sym.symbols('H3, K3, A3, B3, C3, D3')
H3 = -a_1+p_x*C(J_1)+p_y*S(J_1)
K3 = p_z

display(H3)

eq_J3 = -a_2**2 + (-H -a_3*S(J_3)+d_4*C(J_3)) **2 + (K-a_3*C(J_3)-d_4*S(J_3)) **2
sym.collect(sym.trigsimp(sym.expand(eq_J3)), [C(J_3), S(J_3)])

Here is the equation below
 eq3 = 

$\displaystyle a_{1} - a_{2} \sin{\left(J_{2} \right)} - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)} - p_{x} \cos{\left(J_{1} \right)} - p_{y} \sin{\left(J_{1} \right)}$

eq7 = 

$\displaystyle a_{2} \cos{\left(J_{2} \right)} + a_{3} \cos{\left(J_{3} \right)} + d_{4} \sin{\left(J_{3} \right)} - p_{z}$

$\displaystyle - a_{1} + \frac{p_{x}^{2}}{\sqrt{p_{x}^{2} + p_{y}^{2}}} + \frac{p_{y}^{2}}{\sqrt{p_{x}^{2} + p_{y}^{2}}}$

$\displaystyle a_{1}^{2} - \frac{2 a_{1} p_{x}^{2}}{\sqrt{p_{x}^{2} + p_{y}^{2}}} - \frac{2 a_{1} p_{y}^{2}}{\sqrt{p_{x}^{2} + p_{y}^{2}}} - a_{2}^{2} + a_{3}^{2} + d_{4}^{2} + p_{x}^{2} + p_{y}^{2} + p_{z}^{2} + \left(- 2 a_{1} a_{3} + \frac{2 a_{3} p_{x}^{2}}{\sqrt{p_{x}^{2} + p_{y}^{2}}} + \frac{2 a_{3} p_{y}^{2}}{\sqrt{p_{x}^{2} + p_{y}^{2}}} - 2 d_{4} p_{z}\right) \sin{\left(J_{3} \right)} + \left(2 a_{1} d_{4} - 2 a_{3} p_{z} - \frac{2 d_{4} p_{x}^{2}}{\sqrt{p_{x}^{2} + p_{y}^{2}}} - \frac{2 d_{4} p_{y}^{2}}{\sqrt{p_{x}^{2} + p_{y}^{2}}}\right) \cos{\left(J_{3} \right)}$

# eq2 (3.76)
for idx in range(12):
    display(sym.simplify ( sym.expand( sym.Eq( A1iT6[idx], A23456[idx])) ) )

$\displaystyle n_{x} \cos{\left(J_{1} \right)} + n_{y} \sin{\left(J_{1} \right)} = \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} - \sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)} - \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{6} \right)}$

$\displaystyle o_{x} \cos{\left(J_{1} \right)} + o_{y} \sin{\left(J_{1} \right)} = \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{6} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{3} \right)}$

$\displaystyle a_{x} \cos{\left(J_{1} \right)} + a_{y} \sin{\left(J_{1} \right)} = - \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)}$

$\displaystyle - a_{1} + p_{x} \cos{\left(J_{1} \right)} + p_{y} \sin{\left(J_{1} \right)} = - a_{2} \sin{\left(J_{2} \right)} - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)}$

$\displaystyle n_{z} = - \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{3} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)}$

$\displaystyle o_{z} = \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \sin{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{6} \right)} - \sin{\left(J_{6} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}$

$\displaystyle a_{z} = \sin{\left(J_{3} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)}$

$\displaystyle p_{z} = a_{2} \cos{\left(J_{2} \right)} + a_{3} \cos{\left(J_{3} \right)} + d_{4} \sin{\left(J_{3} \right)}$

$\displaystyle n_{x} \sin{\left(J_{1} \right)} - n_{y} \cos{\left(J_{1} \right)} = \sin{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)} + \sin{\left(J_{6} \right)} \cos{\left(J_{4} \right)}$

$\displaystyle o_{x} \sin{\left(J_{1} \right)} - o_{y} \cos{\left(J_{1} \right)} = - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{5} \right)} + \cos{\left(J_{4} \right)} \cos{\left(J_{6} \right)}$

$\displaystyle \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} = a_{x} \sin{\left(J_{1} \right)} - a_{y} \cos{\left(J_{1} \right)}$

$\displaystyle p_{x} \sin{\left(J_{1} \right)} - p_{y} \cos{\left(J_{1} \right)} = 0$

# eq2 (3.76)
sym.simplify ( sym.expand( sym.Eq(
    A1iT6[3]*A1iT6[3] + A1iT6[7]*A1iT6[7],
    A23456[3]* A23456[3] + A23456[7]*A23456[7]) ) )

$\displaystyle a_{2}^{2} + 2 a_{2} a_{3} \cos{\left(J_{2} - J_{3} \right)} - 2 a_{2} d_{4} \sin{\left(J_{2} - J_{3} \right)} + a_{3}^{2} + d_{4}^{2} = a_{1}^{2} - 2 a_{1} p_{x} \cos{\left(J_{1} \right)} - 2 a_{1} p_{y} \sin{\left(J_{1} \right)} + p_{x}^{2} \cos^{2}{\left(J_{1} \right)} + p_{x} p_{y} \sin{\left(2 J_{1} \right)} + p_{y}^{2} \sin^{2}{\left(J_{1} \right)} + p_{z}^{2}$

# eq2 (3.76)
sym.simplify ( sym.expand( sym.Eq(A1iT6[7]*A1iT6[7] , A23456[7]*A23456[7]) ) )

$\displaystyle p_{z}^{2} = a_{2}^{2} \cos^{2}{\left(J_{2} \right)} + 2 a_{2} a_{3} \cos{\left(J_{2} \right)} \cos{\left(J_{3} \right)} + 2 a_{2} d_{4} \sin{\left(J_{3} \right)} \cos{\left(J_{2} \right)} + a_{3}^{2} \cos^{2}{\left(J_{3} \right)} + a_{3} d_{4} \sin{\left(2 J_{3} \right)} + d_{4}^{2} \sin^{2}{\left(J_{3} \right)}$

T36

$\displaystyle \left[\begin{matrix}- n_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - n_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} + n_{z} \cos{\left(J_{3} \right)} & - o_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - o_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} + o_{z} \cos{\left(J_{3} \right)} & - a_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - a_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} + a_{z} \cos{\left(J_{3} \right)} & a_{1} \sin{\left(J_{3} \right)} - a_{2} \cos{\left(J_{2} - J_{3} \right)} - a_{3} - p_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} + p_{z} \cos{\left(J_{3} \right)}\\n_{x} \sin{\left(J_{1} \right)} - n_{y} \cos{\left(J_{1} \right)} & o_{x} \sin{\left(J_{1} \right)} - o_{y} \cos{\left(J_{1} \right)} & a_{x} \sin{\left(J_{1} \right)} - a_{y} \cos{\left(J_{1} \right)} & p_{x} \sin{\left(J_{1} \right)} - p_{y} \cos{\left(J_{1} \right)}\\n_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + n_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + n_{z} \sin{\left(J_{3} \right)} & o_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + o_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + o_{z} \sin{\left(J_{3} \right)} & a_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + a_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + a_{z} \sin{\left(J_{3} \right)} & - a_{1} \cos{\left(J_{3} \right)} + a_{2} \sin{\left(J_{2} - J_{3} \right)} + p_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + p_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + p_{z} \sin{\left(J_{3} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$

A456

$\displaystyle \left[\begin{matrix}- \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} + \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)} & - \sin{\left(J_{4} \right)} \cos{\left(J_{6} \right)} - \sin{\left(J_{6} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} & \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} & 0\\\sin{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)} + \sin{\left(J_{6} \right)} \cos{\left(J_{4} \right)} & - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{5} \right)} + \cos{\left(J_{4} \right)} \cos{\left(J_{6} \right)} & \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} & 0\\- \sin{\left(J_{5} \right)} \cos{\left(J_{6} \right)} & \sin{\left(J_{5} \right)} \sin{\left(J_{6} \right)} & \cos{\left(J_{5} \right)} & d_{4}\\0 & 0 & 0 & 1\end{matrix}\right]$

# eq2 (3.76)
for idx in [3,11]:
    display(sym.simplify ( sym.expand( sym.Eq( T36[idx]*T36[idx], A456[idx]* A456[idx])) ) )

$\displaystyle a_{1}^{2} \sin^{2}{\left(J_{3} \right)} - 2 a_{1} a_{2} \sin{\left(J_{3} \right)} \cos{\left(J_{2} - J_{3} \right)} - 2 a_{1} a_{3} \sin{\left(J_{3} \right)} - 2 a_{1} p_{x} \sin^{2}{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - 2 a_{1} p_{y} \sin{\left(J_{1} \right)} \sin^{2}{\left(J_{3} \right)} + a_{1} p_{z} \sin{\left(2 J_{3} \right)} + a_{2}^{2} \cos^{2}{\left(J_{2} - J_{3} \right)} + 2 a_{2} a_{3} \cos{\left(J_{2} - J_{3} \right)} + 2 a_{2} p_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{2} - J_{3} \right)} + 2 a_{2} p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{2} - J_{3} \right)} - 2 a_{2} p_{z} \cos{\left(J_{3} \right)} \cos{\left(J_{2} - J_{3} \right)} + a_{3}^{2} + 2 a_{3} p_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} + 2 a_{3} p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} - 2 a_{3} p_{z} \cos{\left(J_{3} \right)} + p_{x}^{2} \sin^{2}{\left(J_{3} \right)} \cos^{2}{\left(J_{1} \right)} + 2 p_{x} p_{y} \sin{\left(J_{1} \right)} \sin^{2}{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - \frac{p_{x} p_{z} \left(- \sin{\left(J_{1} - 2 J_{3} \right)} + \sin{\left(J_{1} + 2 J_{3} \right)}\right)}{2} + p_{y}^{2} \sin^{2}{\left(J_{1} \right)} \sin^{2}{\left(J_{3} \right)} - \frac{p_{y} p_{z} \left(\cos{\left(J_{1} - 2 J_{3} \right)} - \cos{\left(J_{1} + 2 J_{3} \right)}\right)}{2} + p_{z}^{2} \cos^{2}{\left(J_{3} \right)} = 0$

$\displaystyle d_{4}^{2} = a_{1}^{2} \cos^{2}{\left(J_{3} \right)} - 2 a_{1} a_{2} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{3} \right)} - 2 a_{1} p_{x} \cos{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} - 2 a_{1} p_{y} \sin{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} - a_{1} p_{z} \sin{\left(2 J_{3} \right)} + a_{2}^{2} \sin^{2}{\left(J_{2} - J_{3} \right)} + 2 a_{2} p_{x} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + 2 a_{2} p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{3} \right)} + 2 a_{2} p_{z} \sin{\left(J_{3} \right)} \sin{\left(J_{2} - J_{3} \right)} + p_{x}^{2} \cos^{2}{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} + 2 p_{x} p_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} + \frac{p_{x} p_{z} \left(- \sin{\left(J_{1} - 2 J_{3} \right)} + \sin{\left(J_{1} + 2 J_{3} \right)}\right)}{2} + p_{y}^{2} \sin^{2}{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} + \frac{p_{y} p_{z} \left(\cos{\left(J_{1} - 2 J_{3} \right)} - \cos{\left(J_{1} + 2 J_{3} \right)}\right)}{2} + p_{z}^{2} \sin^{2}{\left(J_{3} \right)}$

# eq2 (3.76)
display(sym.trigsimp ( sym.expand( sym.Eq( T36[3]*T36[3] - T36[11]*T36[11], A456[3]* A456[3]-A456[11]* A456[11])) ) )

$\displaystyle a_{1}^{2} \sin^{2}{\left(J_{3} \right)} - a_{1}^{2} \cos^{2}{\left(J_{3} \right)} - 2 a_{1} a_{2} \sin{\left(J_{3} \right)} \cos{\left(J_{2} - J_{3} \right)} + 2 a_{1} a_{2} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{3} \right)} - 2 a_{1} a_{3} \sin{\left(J_{3} \right)} - 2 a_{1} p_{x} \sin^{2}{\left(J_{3} \right)} \cos{\left(J_{1} \right)} + 2 a_{1} p_{x} \cos{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} - 2 a_{1} p_{y} \sin{\left(J_{1} \right)} \sin^{2}{\left(J_{3} \right)} + 2 a_{1} p_{y} \sin{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} + 2 a_{1} p_{z} \sin{\left(2 J_{3} \right)} - a_{2}^{2} \sin^{2}{\left(J_{2} - J_{3} \right)} + a_{2}^{2} \cos^{2}{\left(J_{2} - J_{3} \right)} + 2 a_{2} a_{3} \cos{\left(J_{2} - J_{3} \right)} + 2 a_{2} p_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{2} - J_{3} \right)} - 2 a_{2} p_{x} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + 2 a_{2} p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{2} - J_{3} \right)} - 2 a_{2} p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{3} \right)} - 2 a_{2} p_{z} \sin{\left(J_{3} \right)} \sin{\left(J_{2} - J_{3} \right)} - 2 a_{2} p_{z} \cos{\left(J_{3} \right)} \cos{\left(J_{2} - J_{3} \right)} + a_{3}^{2} + 2 a_{3} p_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} + 2 a_{3} p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} - 2 a_{3} p_{z} \cos{\left(J_{3} \right)} + p_{x}^{2} \sin^{2}{\left(J_{3} \right)} \cos^{2}{\left(J_{1} \right)} - p_{x}^{2} \cos^{2}{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} + 2 p_{x} p_{y} \sin{\left(J_{1} \right)} \sin^{2}{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - 2 p_{x} p_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} - p_{x} p_{z} \left(- \sin{\left(J_{1} - 2 J_{3} \right)} + \sin{\left(J_{1} + 2 J_{3} \right)}\right) + p_{y}^{2} \sin^{2}{\left(J_{1} \right)} \sin^{2}{\left(J_{3} \right)} - p_{y}^{2} \sin^{2}{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} - p_{y} p_{z} \left(\cos{\left(J_{1} - 2 J_{3} \right)} - \cos{\left(J_{1} + 2 J_{3} \right)}\right) - p_{z}^{2} \sin^{2}{\left(J_{3} \right)} + p_{z}^{2} \cos^{2}{\left(J_{3} \right)} = - d_{4}^{2}$

# eq2 (3.76)
idx=6
sym.Eq(A456[idx] , T36[idx])

$\displaystyle \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} = a_{x} \sin{\left(J_{1} \right)} - a_{y} \cos{\left(J_{1} \right)}$

sym.trigsimp(             A1*A2*A3*A4)

$\displaystyle \left[\begin{matrix}\sin{\left(J_{1} \right)} \sin{\left(J_{4} \right)} - \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} & - \frac{\cos{\left(J_{1} - J_{3} \right)}}{2} - \frac{\cos{\left(J_{1} + J_{3} \right)}}{2} & \sin{\left(J_{1} \right)} \cos{\left(J_{4} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)} & \left(a_{1} - a_{2} \sin{\left(J_{2} \right)} - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)}\right) \cos{\left(J_{1} \right)}\\- \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} - \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)} & - \frac{\sin{\left(J_{1} - J_{3} \right)}}{2} - \frac{\sin{\left(J_{1} + J_{3} \right)}}{2} & \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} - \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} & \left(a_{1} - a_{2} \sin{\left(J_{2} \right)} - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)}\right) \sin{\left(J_{1} \right)}\\\cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} & - \sin{\left(J_{3} \right)} & - \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)} & a_{2} \cos{\left(J_{2} \right)} + a_{3} \cos{\left(J_{3} \right)} + d_{4} \sin{\left(J_{3} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$

sym.trigsimp(            A5i*A6i*T6)

$\displaystyle \left[\begin{matrix}n_{x} \cos{\left(J_{5} + J_{6} \right)} + n_{y} \sin{\left(J_{5} + J_{6} \right)} & o_{x} \cos{\left(J_{5} + J_{6} \right)} + o_{y} \sin{\left(J_{5} + J_{6} \right)} & a_{x} \cos{\left(J_{5} + J_{6} \right)} + a_{y} \sin{\left(J_{5} + J_{6} \right)} & p_{x} \cos{\left(J_{5} + J_{6} \right)} + p_{y} \sin{\left(J_{5} + J_{6} \right)}\\n_{z} & o_{z} & a_{z} & p_{z}\\n_{x} \sin{\left(J_{5} + J_{6} \right)} - n_{y} \cos{\left(J_{5} + J_{6} \right)} & o_{x} \sin{\left(J_{5} + J_{6} \right)} - o_{y} \cos{\left(J_{5} + J_{6} \right)} & a_{x} \sin{\left(J_{5} + J_{6} \right)} - a_{y} \cos{\left(J_{5} + J_{6} \right)} & p_{x} \sin{\left(J_{5} + J_{6} \right)} - p_{y} \cos{\left(J_{5} + J_{6} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$